/* 
 * File:   PID.h
 * Author: Rolf
 *
 * Created on 22. janvier 2014, 22:39
 */
#include "user.h"
#include "EEPROM.h"
#include "PID.h"

long count = 0;
long last_count = 0;

#define   GUARD_GAIN   10.0                // 20.0

float K =  DEFAULT_K;   //1.5;  // wheels 80mm // 1.9 for 100mm wheels
float Kp = DEFAULT_KP;  // 5;   //3.6;//7.2;  //7
float Ki = DEFAULT_KI;  //1.3;  // 1.3;//4.6;  //1
float Kd = DEFAULT_KD;  //-1.8; // -0.80;  //-6

float last_error = 0;
float integrated_error = 0;
float pTerm=0, iTerm=0, dTerm=0, pTerm_Wheel=0, dTerm_Wheel=0;

int constrain(int x,int y, int z){
    if(x<=z && x>=y) return x;
    if(x>z)return z;
    if(x<y) return y;
}

int updatePid(int targetPosition, int currentPosition)   {
  int error = targetPosition - currentPosition;
  pTerm = Kp * error;
  integrated_error += error;
  iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
  dTerm = Kd * (error - last_error);
  last_error = error;
  return -constrain(K*(pTerm + iTerm + dTerm), -245, 245);
}

void ResetPID(){
  last_error = 0;
  last_count = 0;
  count = 0;
  integrated_error = 0;
}
